#ifndef FSM_STATE_TEST_H
#define FSM_STATE_TEST_H

#include "FSM_State.h"

struct DogModel
{
    Vec3<float> OQ_init;
    Vec3<float> OP_init;
    Vec3<float> QP_O;
    Vec3<float> QP_Q;
    Vec3<float> OQ_now;
};



template <typename T>
class FSM_State_Test : public FSM_State<T> {
 public:
  FSM_State_Test(ControlFSMData<T>* _controlFSMData);

  // Behavior to be carried out when entering a state
  void onEnter();

  // Run the normal behavior for the state
  void run();

  // Checks for any transition triggers
  FSM_StateName checkTransition();

  // Manages state specific transitions
  TransitionData<T> transition();

  // Behavior to be carried out when exiting a state
  void onExit();

  void cal_du(float roll,float pitch,float yaw);
  void cal_hu(float roll,float pitch,float yaw);

 private:
  // Keep track of the control iterations
  Mat3<float> Rot;
  DogModel _dogModel[4];
  float length=0.522;
  float width=0.290;
  float high=-0.28;
  int iter = 0;
};




#endif  // FSM_STATE_TEST_H
